# 导入模块
import threading
import time
import serial
import serial.tools.list_ports
import math
import matplotlib.pyplot as plt
import numpy as np
import threading
import queue
import cmath
import open3d as o3d
import keyboard

# import constants as cst
# 自定义变量
port = "COM5"  # 端口号，根据自己实际情况输入，可以在设备管理器查看
bps = 115200     # 串口波特率，根据自己实际情况输入
timeout = 5       # 超时时间,None：永远等待操作，0为立即返回请求结果，其他值为等待超时时间(单位为秒）
rxdata = ''    # 接收的数据
rx_data=[]
rx_buf=[]
rx_buf_show=[]
rx_buf_fin=[]
M_PI = 3.1415926
Angle_Fin = []
Distance_Fin = []
run_flag = False

YawAngle=0

def check_uart_port():
    port_list = list(serial.tools.list_ports.comports())
    # print(port_list)
    if len(port_list) == 0:
        print('can not fine uart port')
        return False
    else:
        for i in range(0,len(port_list)):
            print(port_list[i])
    return True

# 打开串口
def open_uart(port, bps, timeout):
    try:
        # 打开串口，并返回串口对象
        uart = serial.Serial(port, bps, timeout=timeout)
        return uart
    except Exception as result:
        print("can not open uart")
        print(result)
        return False

# 发送数据
def uart_send_data(uart, txbuf):
    len = uart.write(txbuf.encode('utf-8'))  # 写数据
    return len

# 接收数据
def uart_receive_data(uart):
    if uart.in_waiting:
        # rxdata = uart.read(uart.in_waiting).decode("utf-8")   # 以字符串接收
        rxdata = uart.read().hex()  # 以16进制(hex)接收
        # print("rxdata:",type(rxdata),rxdata)  # 打印数据
        # rx_data.append(rxdata)
        # print("rxdata:",rxdata)
        lidarx4_handle(rxdata)


def lidarx4_handle(rx):
    rx_buf_show.append(rx)
    if rx=='aa' and len(rx_buf)<1:
        rx_buf.append(rx)
    elif len(rx_buf)>0 and rx_buf[0]=='aa' and rx=='55':
        rx_buf.append(rx)
        if rx_buf[-2]=='aa':
            if len(rx_buf)>2:
                rx_buf_fin=rx_buf[:-2]
                # print("rx_buf:",rx_buf_fin)

                # print("rx_buf_fin_len:", len(rx_buf_fin))
                # print("rx_buf_fin:", rx_buf_fin)
                # change_c(rx_buf_fin)#转化为c语言的数组格式
                try:
                    usartdata.put(rx_buf_fin)
                    # Lidarx4_data_process_task(rx_buf_fin)
                except:
                    print("error2")
                    pass
                # print("rx_buf_show:",rx_buf_show)
                rx_buf_show.clear()
                rx_buf.clear()
                rx_buf.append('aa')
                rx_buf.append('55')
    elif len(rx_buf)>1 and rx_buf[0]=='aa'  and rx_buf[1]=='55':
        rx_buf.append(rx)


    # print("rx_buf_fin:", rx_buf_fin)
def Lidarx4_data_process_task(usartdata):
    global Angle_Fin
    global Distance_Fin
    # print(data[2])
    while True:
        Buffer=usartdata.get()
        # print("Data len:",len(Buffer))
        if len(Buffer)>9 and Buffer[2]=='00' :
            check_code = 0x55AA

            CT = int(Buffer[2],16) & 0x01
            LSN = int(Buffer[3],16)
            # print("LSC:", LSN)
            FSA = (int(Buffer[5],16) << 8 | int(Buffer[4],16))
            check_code ^= FSA

            LSA = (int(Buffer[7],16) << 8 | int(Buffer[6],16))
            CS = (int(Buffer[9],16) << 8 | int(Buffer[8],16))

            Distance = [0 for x in range(0, LSN)]
            Itensity = [0 for x in range(0, LSN)]
            if LSN==len(Buffer[10:])//3:
                for i in range(0,LSN*3,3):
                    check_code ^= int(Buffer[10 + i],16)
                    # print("i:",i,"Buffer:",int(Buffer[10 + i + 2],16),int(Buffer[10 + i + 1],16))
                    data =(int(Buffer[10 + i + 2],16) << 8 | int(Buffer[10 + i + 1],16))
                    check_code ^= data

                    Itensity[i // 3] =((int(Buffer[10 + i + 1],16) & 0x03) << 8 | int(Buffer[10 + i],16))
                    Distance[i // 3] = data >> 2
                    # Distance_Fin.append(Distance[i // 3])

                check_code ^= (LSN << 8 | CT)
                check_code ^= LSA

                Angle = [0 for x in range(0, LSN)]

                if (check_code == CS):
                    Angle_FSA = (FSA >> 1) // 64
                    Angle_LSA = (LSA >> 1) // 64
                    Angle_Diff = (Angle_LSA - Angle_FSA)
                    if (Angle_Diff < 0):
                        Angle_Diff = Angle_Diff + 360
                    for i in range(0,LSN):
                        if (LSN > 1):
                            Angle[i] = i * Angle_Diff / (LSN - 1) + Angle_FSA
                        else:
                            Angle[i] = Angle_FSA

                        if (Distance[i] > 0):
                            AngCorrect =math.atan(21.8 * (155.3 - Distance[i]) / (155.3 * Distance[i]))
                            Angle[i] = Angle[i] + AngCorrect * 180 / M_PI

                        Angle[i]=math.radians(Angle[i])
                        # Angle_Fin.append(round(Angle[i],6))
                    # plt_show(Angle, Distance)
                    # print("Distance_len:",len(Distance),"Angle_len:",len(Angle))
                    lidardata.put([Angle,Distance])
                    # print(Distance_Fin,Angle_Fin)

                else:
                    print("error")

def Lidarx4_data_show_task(lidardata):

    vis = o3d.visualization.Visualizer()
    # vis.create_window(width=800, height=600)
    vis.create_window()
    pointcloud = o3d.geometry.PointCloud()
    mesh_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=1000, origin=[0, 0, 0])
    to_reset = True
    vis.add_geometry(pointcloud)
    # vis.VisualizerWithKeyCallback.register_key_callback(90, lambda temp: showrun())
    x=[]
    y=[]
    points=[]
    colors=[]
    while True:
        try:
            # print("in1")
            # tem_data = lidardata.get()
            tem_data = lidardata.get_nowait()
            # print("showdata get")
            Distance_Show = tem_data[1]
            Angle_Show = tem_data[0]
            for i in range(len(Angle_Show)):
                if 0<math.degrees(Angle_Show[i])<135 or math.degrees(Angle_Show[i])>270-45:
                    continue
                rec_xyz=cmath.rect(Distance_Show[i],Angle_Show[i])
                # x.append(rec_xyz.real)
                # y.append( rec_xyz.imag)

                #正常
                # x=rec_xyz.real
                # y=rec_xyz.imag
                # z=0

                z=-rec_xyz.real
                y=rec_xyz.imag*math.cos(math.radians(YawAngle))
                x=rec_xyz.imag*math.sin(math.radians(YawAngle))
                f.write(str(x)+' '+str(y)+' '+str(z)+"\n")
                # print(x,y,z)
                #给点云距离限制距离阈值，如果一开始读取到近的点，后面远的点因为窗口无法无限缩小而看不到
                if abs(x)>1000 or abs(y)>1000 or abs(z)>1000:
                    continue
                r=1
                g=0
                b=0
                points.append([x,y,z])
                colors.append([r,g,b])
            # points.scale(2.0)
            # pointcloud = o3d.geometry.PointCloud()
            pointcloud.points = o3d.utility.Vector3dVector(points)
            pointcloud.colors = o3d.utility.Vector3dVector(colors)
            # points = []
            # colors = []
            # pointcloud=pointcloud/2
            # points.clear()
            # colors.clear()
            vis.update_geometry(pointcloud)
            vis.add_geometry(pointcloud)
            if to_reset:
                vis.reset_view_point(True)
                to_reset = False
            vis.poll_events()
            vis.update_renpderer()

        except :#queue.Empty:
            if run_flag ==True:
                print("vis run")
                # vis.reset_view_point(True)
                vis.run()

                print("end")
                while True:
                    # time.sleep(1)
                    pass
            # print("error")
            time.sleep(0.000001)
            continue



def showrun_flag():
    global run_flag
    run_flag=True
    f.close()
    print("stop update")
    # Lidarx4_data_show_task.vis.run()
    # vis.run()

def change_degree():
    global YawAngle
    print("change")
    YawAngle+=45

def pointcloud_show_all(points,colors):
    vis = o3d.visualization.Visualizer()
    vis.create_window()

    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(points)
    pcd.colors = o3d.utility.Vector3dVector(colors)

    vis.add_geometry(pcd)
    vis.poll_events()
    vis.update_renderer()
    vis.run()

def change_c(data):
    for i in range(len(data)):
        print("Buffer["+str(i)+"]=0X"+data[i]+";")
# 关闭串口
def close_uart(uart):
    uart.close()



def usarttask():
    # 扫描端口
    result = check_uart_port()
    if(result == False):
        return

    # 打开串口
    result = open_uart(port, bps, timeout)
    if (result == False):
        return
    else:
        uart1 = result
    while True:
        # 串口接收数据方法3
        uart_receive_data(uart1)  # 接收数据



if __name__ == '__main__':
    print("Start")
    f=open("./data/welddefect.txt","w")
    keyboard.add_hotkey('q', showrun_flag)
    keyboard.add_hotkey('w', change_degree)
    usartdata = queue.Queue()
    lidardata = queue.Queue()

    thread_usart=threading.Thread(target=usarttask)
    thread_lidardataprocess=threading.Thread(target=Lidarx4_data_process_task,args=(usartdata,))
    thread_lidardatashow=threading.Thread(target=Lidarx4_data_show_task,args=(lidardata,))

    thread_usart.start()
    thread_lidardataprocess.start()
    thread_lidardatashow.start()


# print(math.radians(30))
# plt_show([0.523,0.523],[5,10])
# DATA=['aa', '55', '00', '01', '53', 'ae', '53', 'ae', 'ab', '54', '00', '00', '00']#, 'cc', 'e5', '6f', 'fd', 'eb', '01', '00', '00', '00', 'fd', 'f3', '01', '00', '00', '00', 'fd', '03', '02', '00', '00', '00', 'fd', '1b', '02', '00', '00', '00', 'fd', '2f', '02', '00', '00', '00', 'fd', '43', '02', '00', '00', '00', 'fd', '5b', '02', '00', '00', '00', '60', '73', '02', '00', '00', '00', '02', '86', '02', '00', '00', '00', 'fe', '67', '02', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', 'fe', '8b', '02', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00']
# DATA=['aa', '55', '00', '28', 'e5', '6f', 'bd', '79', '37', '6a', 'a0', '0f', '00', '00', '00', '00', 'fd', 'eb', '01', '00', '00', '00', 'fd', 'f3', '01', '00', '00', '00', 'fd', '03', '02', '00', '00', '00', 'fd', '1b', '02', '00', '00', '00', 'fd', '2f', '02', '00', '00', '00', 'fd', '43', '02', '00', '00', '00', 'fd', '5b', '02', '00', '00', '00', '60', '73', '02', '00', '00', '00', '02', '86', '02', '00', '00', '00', 'fe', '67', '02', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', 'fe', '8b', '02', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00', '00']
# Lidarx4_data_process(DATA)
